/*
 * HidFFB.cpp
 *
 *  Created on: 12.02.2020
 *      Author: Yannick
 */

/*
	Different types of conditions will interpret the parameters differently, but the basic idea is that force
	resulting from a condition is equal to A(q - q0) where A is a scaling coefficient, q is some metric, and q0
	is the neutral value for that metric.
	The simplified formula given above must be adjusted if a nonzero dead band is provided. If the metric is
	less than CP Offset - Dead Band, then the resulting force is given by the following formula:
	force = Negative Coefficient * (q - (CP Offset �C Dead Band))
	Similarly, if the metric is greater than CP Offset + Dead Band, then the resulting force is given by the
	following formula:
	force = Positive Coefficient * (q - (CP Offset + Dead Band))
	A spring condition uses axis position as the metric.
	A damper condition uses axis velocity as the metric.
	An inertia condition uses axis acceleration as the metric.
	If the number of Condition report blocks is equal to the number of axes for the effect, then the first report
	block applies to the first axis, the second applies to the second axis, and so on. For example, a two-axis
	spring condition with CP Offset set to zero in both Condition report blocks would have the same effect as
	the joystick self-centering spring. When a condition is defined for each axis in this way, the effect must
	not be rotated.
	If there is a single Condition report block for an effect with more than one axis, then the direction along
	which the parameters of the Condition report block are in effect is determined by the direction parameters
	passed in the Direction field of the Effect report block. For example, a friction condition rotated 45
	degrees (in polar coordinates) would resist joystick motion in the northeast-southwest direction but would
	have no effect on joystick motion in the northwest-southeast direction.
 */

#include <assert.h>
#include "HidFFB.h"
#include "eeprom.h"
#include "eeprom_addresses.h"
#include "main.h"
#include "Leds.h"
#include "usbd_hid_custom_if.h"
#define DBG_TAG "hidffb"
//#define DBG_LVL DBG_LOG
#define DBG_LVL DBG_ERROR
#include <rtdbg.h>

EffectsCalculator	etCalcs = {	.calcfrequency = 1000,\
								.qfloatScaler = 0.01,\
								.filter[0].constant.fc = 500,\
								.filter[0].constant.q = 70,\
								.filter[0].friction.fc = 500,\
								.filter[0].friction.q = 20,\
								.filter[0].damper.fc = 30,\
								.filter[0].damper.q = 40,\
								.filter[0].inertia.fc = 15,\
								.filter[0].inertia.q = 20,\
								.gain.friction = 255,\
								.gain.spring = 255,\
								.gain.damper = 64,\
								.gain.inertia = 127,\
								.gain.periodic = 255,\
								.scaler.friction = 1.0,\
								.scaler.spring = 16.0,\
								.scaler.damper = 4.0,\
								.scaler.inertia = 2.0,\
								.scaler.periodic = 1.0,\
								.global_gain = 255,\
	
								};

bool HidFFB_HID_SendReport(HidFFB* ffb,uint8_t *report,uint16_t len);
uint32_t HidFFB_getRate(HidFFB* ffb);
uint32_t HidFFB_getConstantForceRate(HidFFB* ffb);
void HidFFB_sendStatusReport(HidFFB* ffb,uint8_t effect);
void HidFFB_hidOut(HidFFB* ffb,uint8_t report_id, uint8_t const* buffer, uint16_t bufsize);

void HidFFB_free_effect(HidFFB* ffb,uint16_t idx);
uint16_t HidFFB_hidGet(HidFFB* ffb,uint8_t report_id,uint8_t* buffer, uint16_t reqlen);

void HidFFB_start_FFB(HidFFB* ffb);

void HidFFB_stop_FFB(HidFFB* ffb);

void HidFFB_set_FFB(HidFFB* ffb,bool state);

void HidFFB_set_gain(HidFFB* ffb,uint8_t gain);

void HidFFB_set_filters(HidFFB* ffb,FFB_Effect *effect);

void HidFFB_ffb_control(HidFFB* ffb,uint8_t cmd);

void HidFFB_set_constant_effect(HidFFB* ffb,FFB_SetConstantForce_Data_t* data);

void HidFFB_new_effect(HidFFB* ffb,FFB_CreateNewEffect_Feature_Data_t* effect);

void HidFFB_set_effect(HidFFB* ffb,FFB_SetEffect_t* effect);

void HidFFB_set_condition(HidFFB* ffb,FFB_SetCondition_Data_t *cond);

void HidFFB_set_effect_operation(HidFFB* ffb,FFB_EffOp_Data_t* report);

void HidFFB_set_envelope(HidFFB* ffb,FFB_SetEnvelope_Data_t *report);

void HidFFB_set_ramp(HidFFB* ffb,FFB_SetRamp_Data_t *report);

void HidFFB_set_periodic(HidFFB* ffb,FFB_SetPeriodic_Data_t* report);

uint8_t HidFFB_find_free_effect(HidFFB* ffb,uint8_t type);

void HidFFB_reset_ffb(HidFFB* ffb);


void HidFFB_Init(HidFFB* ffb)
{
	// Initialize reports
	ffb->used_effects = 0;
	ffb->blockLoad_report.reportId = HID_ID_BLKLDREP;
	ffb->blockLoad_report.effectBlockIndex = 1;
	ffb->blockLoad_report.ramPoolAvailable = MAX_EFFECTS*sizeof(FFB_Effect);
	ffb->blockLoad_report.loadStatus = BLOCK_LOAD_SUCCESS;

	ffb->pool_report.reportId = HID_ID_POOLREP;
	ffb->pool_report.ramPoolSize = MAX_EFFECTS*sizeof(FFB_Effect);
	ffb->pool_report.maxSimultaneousEffects = MAX_EFFECTS;
	ffb->pool_report.memoryManagement = DEVICE_MANAGED_POOL;
	
	ffb->reportFFBStatus.reportId = HID_ID_STATE;
	
	HidFFB_setEffectsCalculator(ffb,&etCalcs);
}


void HidFFB_setEffectsCalculator(HidFFB* ffb,EffectsCalculator *ec) {
	ffb->effects_calc = ec;
	assert(ffb->effects_calc != NULL);
	EC_restoreFlash(ffb->effects_calc);		// �ָ���Ч�������
	EC_setEffectsArray(ffb->effects_calc,ffb->effects);
	EC_setActive(ffb->effects_calc,ffb->ffb_active);
}


bool HidFFB_getFfbActive(HidFFB* ffb){
	return ffb->ffb_active;
}

bool HidFFB_HID_SendReport(HidFFB* ffb,uint8_t *report,uint16_t len){
	return CUSTOM_HID_SendReport(0, report, len);
}

/*
	Sends a status report for a specific effect
 */
void HidFFB_sendStatusReport(HidFFB* ffb,uint8_t index)
{
	ffb->reportFFBStatus.reportId = HID_ID_STATE;
	ffb->reportFFBStatus.effectBlockIndex = index;
	ffb->reportFFBStatus.status = HID_ACTUATOR_POWER;
	if(ffb->ffb_active)
	{
		ffb->reportFFBStatus.status |= HID_ENABLE_ACTUATORS;
		ffb->reportFFBStatus.status |= HID_EFFECT_PLAYING;
	}
	else
	{
		ffb->reportFFBStatus.status |= HID_EFFECT_PAUSE;
	}
	HidFFB_HID_SendReport(ffb,(uint8_t*)(&ffb->reportFFBStatus), sizeof(reportFFB_status_t));
}


/**
 * Called when HID OUT data is received via USB
 */
void HidFFB_hidOut(HidFFB* ffb,uint8_t report_id, uint8_t const* buffer, uint16_t bufsize)
{
	static uint32_t gameDataCnt = 0; 
	if(gameDataCnt++ > 100)
	{
		gameDataCnt = 0;
		blinkLed2(50,0x1);
	}
	// FFB Output Message
	const uint8_t* report = buffer;
	uint8_t event_idx = report_id - FFB_ID_OFFSET;
	if(event_idx == 0){
	    return;
	}
	LOG_D("report_id:%d\t\t",event_idx);
	// -------- Out Reports --------
	switch(event_idx)
	{
		case HID_ID_NEWEFREP: //add Effect Report. Feature
			HidFFB_new_effect(ffb,(FFB_CreateNewEffect_Feature_Data_t*)(report));
			LOG_D("new_effect,type:%x\r\n",report[1]);
			break;

		case HID_ID_EFFREP: // Set Effect
		{
			FFB_SetEffect_t setEffectRepBuf;
			memcpy(&setEffectRepBuf,report,sizeof(FFB_SetEffect_t)<bufsize?sizeof(FFB_SetEffect_t):bufsize); // Copy report to buffer. only valid range if less axes are used
			HidFFB_set_effect(ffb,&setEffectRepBuf);
			LOG_D("Set Effect\r\n");
			break;
		}
		case HID_ID_CTRLREP: // Control report. 1=Enable Actuators, 2=Disable Actuators, 4=Stop All Effects, 8=Reset, 16=Pause, 32=Continue
			HidFFB_ffb_control(ffb,report[1]);
			LOG_D("Control report:%x\r\n",report[1]);
			break;

		case HID_ID_GAINREP: // Set global gain
			HidFFB_set_gain(ffb,report[1]);
			LOG_D("Set global gain:%x\r\n",report[1]);
			break;

		case HID_ID_ENVREP: // Envelope
			HidFFB_set_envelope(ffb,(FFB_SetEnvelope_Data_t *)report);
			LOG_D("Set Envelope\r\n");
			break;

		case HID_ID_CONDREP: // Spring, Damper, Friction, Inertia
			HidFFB_set_condition(ffb,(FFB_SetCondition_Data_t*)report);
			LOG_D("Set Condition\r\n");
			break;

		case HID_ID_PRIDREP: // Periodic
			HidFFB_set_periodic(ffb,(FFB_SetPeriodic_Data_t*)report);
			LOG_D("Set periodic\r\n");
			break;

		case HID_ID_CONSTREP: // Constant
			HidFFB_set_constant_effect(ffb,(FFB_SetConstantForce_Data_t*)report);
			LOG_D("Set constant\r\n");
			break;

		case HID_ID_RAMPREP: // Ramp
			HidFFB_set_ramp(ffb,(FFB_SetRamp_Data_t *)report);
			LOG_D("Set ramp\r\n");
			break;

		case HID_ID_CSTMREP: // Custom. pretty much never used
			LOG_D("Set custom\r\n");
			break;

		case HID_ID_SMPLREP: // Download sample
			LOG_D("Download sample\r\n");
			break;

		case HID_ID_EFOPREP: //Effect operation
			HidFFB_set_effect_operation(ffb,(FFB_EffOp_Data_t*)report);
			LOG_D("Effect operation,reportId:%x;\t effectBlockIndex:%x \t state:%x \t loopCount:%x\r\n",report[0],report[1],report[2],report[3]);
			break;

		case HID_ID_BLKFRREP: // Free a block
			HidFFB_free_effect(ffb,report[1]-1);
			LOG_D("Free a block\r\n");
			break;

		case HID_ID_CUSTOMCMD:
//			HidFFB_free_effect(ffb,report[1]-1);
			break;

		default:
			break;

	}

}


/**
 * Called on HID feature GET events
 * Any reply is assigned to the return buffer
 *
 * Handles block load reports and pool status which are requested after a new effect has been created
 */
uint16_t HidFFB_hidGet(HidFFB* ffb,uint8_t report_id,uint8_t* buffer, uint16_t reqlen){
	// Feature gets go here

	uint8_t id = report_id - FFB_ID_OFFSET;

	switch(id){
	case HID_ID_BLKLDREP:
	    LOG_D("get id %d",id);
		// Notice: first byte ID is not present in the reply buffer because it is handled by tinyusb internally!
	    ffb->blockLoad_report.reportId = HID_ID_BLKLDREP;
		memcpy(buffer,&ffb->blockLoad_report,sizeof(FFB_BlockLoad_Feature_Data_t));
		return sizeof(FFB_BlockLoad_Feature_Data_t);
		break;
	case HID_ID_POOLREP:
	    LOG_D("get id %d",id);
	    ffb->pool_report.reportId = HID_ID_POOLREP;
		memcpy(buffer,&ffb->pool_report,sizeof(FFB_PIDPool_Feature_Data_t));
		return sizeof(FFB_PIDPool_Feature_Data_t);
		break;
	default:
		break;
	}
	return 0;
}

/*
	PID Block Free Report
 */
void HidFFB_free_effect(HidFFB* ffb,uint16_t idx)
{
	if(idx < MAX_EFFECTS)
	{
//		effects_calc->logEffectType(effects[idx].type, true); // Effect off
		if(ffb->effects[idx].type != FFB_EFFECT_NONE)
		{
			memset(&ffb->effects[idx],0,sizeof(FFB_Effect));
			ffb->used_effects--;
		}
	}

}

void HidFFB_start_FFB(HidFFB* ffb){
	HidFFB_set_FFB(ffb,true);
}

void HidFFB_stop_FFB(HidFFB* ffb){

	HidFFB_set_FFB(ffb,false);
}

void HidFFB_set_FFB(HidFFB* ffb,bool state)
{
	assert(ffb->effects_calc != NULL);
	ffb->ffb_active = state;
	EC_setActive(ffb->effects_calc,state);
}

/*	
	usage Device Gain Report 
*/
void HidFFB_set_gain(HidFFB* ffb,uint8_t gain){
	assert(ffb->effects_calc != NULL);
	EC_setGain(ffb->effects_calc,gain);
}

void HidFFB_set_filters(HidFFB* ffb,FFB_Effect *effect){
	assert(ffb->effects_calc != NULL);
	EC_setFilters(ffb->effects_calc,effect);
}

/*	
	PID Device Control Report 
 */
void HidFFB_ffb_control(HidFFB* ffb,uint8_t cmd){
	
	/* DC Enable Actuators */
	if(cmd & 0x01)
	{
		HidFFB_start_FFB(ffb);
	}
	
	/* DC Disable Actuators */
	if(cmd & 0x02)
	{
		HidFFB_stop_FFB(ffb);
	}
	
	/* DC Stop All Effects:Issues a stop on every running effect */
	if(cmd & 0x04)
	{
		HidFFB_stop_FFB(ffb);
		//start_FFB();
	}
	
	/* DC Device Reset:	Clears any device paused condition, 
	 * enables all actuators and clears all effects from memory */
	if(cmd & 0x08)
	{
		//ffb_active = true;
		HidFFB_stop_FFB(ffb);
		HidFFB_reset_ffb(ffb);
		// reset effects
	}
	
	/* DC Device Pause */
	if(cmd & 0x10)
	{
		HidFFB_stop_FFB(ffb);
	}
	
	/* DC Device Continue */
	if(cmd & 0x20)
	{
		HidFFB_start_FFB(ffb);
	}

	HidFFB_sendStatusReport(ffb,0);
}

/*
	Set Constant Force Report
 */
void HidFFB_set_constant_effect(HidFFB* ffb,FFB_SetConstantForce_Data_t* data){
	if(data->effectBlockIndex == 0 || data->effectBlockIndex > MAX_EFFECTS){
		return;
	}
	FFB_Effect* effect_p = &ffb->effects[data->effectBlockIndex-1];

	effect_p->magnitude = data->magnitude;
	if(effect_p->state == 0)
	{
		effect_p->state = 1; // Force start effect
	}
}

/*
	Create New Effect Report
 */
void HidFFB_new_effect(HidFFB* ffb,FFB_CreateNewEffect_Feature_Data_t* effect){
	
	// Allocates a new effect
	uint8_t index = HidFFB_find_free_effect(ffb,effect->effectType); // next effect
	if(index == 0)
	{
		ffb->blockLoad_report.loadStatus = BLOCK_LOAD_FULL;
		return;
	}

//	EC_logEffectType(ffb->effects_calc,effect->effectType,false);
//	FFB_Effect new_effect;
//	new_effect.type = effect->effectType;
//	EC_setFilters(ffb->effects_calc,&new_effect);
	ffb->used_effects++;
	ffb->effects[index-1].type = effect->effectType;
	// Set block load report
	//reportFFBStatus.effectBlockIndex = index;
	ffb->blockLoad_report.effectBlockIndex = index;
	ffb->blockLoad_report.ramPoolAvailable = (MAX_EFFECTS-ffb->used_effects)*sizeof(FFB_Effect);
	ffb->blockLoad_report.loadStatus = BLOCK_LOAD_SUCCESS;
	
	HidFFB_set_filters(ffb,&ffb->effects[index-1]);

//	HidFFB_sendStatusReport(ffb,index);
	

}

/*
	Set Effect Report
 */
void HidFFB_set_effect(HidFFB* ffb,FFB_SetEffect_t* effect){
	uint8_t index = effect->effectBlockIndex;
	if(index > MAX_EFFECTS || index == 0)
		return;

	FFB_Effect* pEffect = &ffb->effects[index-1];

	if (pEffect->type != effect->effectType){
		pEffect->startTime = 0;
		HidFFB_set_filters(ffb,pEffect);
	}

	pEffect->gain = effect->gain;
	pEffect->type = effect->effectType;
	pEffect->samplePeriod = effect->samplePeriod;

	pEffect->enableAxis = effect->enableAxis;
	pEffect->directionX = effect->directionX;
	pEffect->directionY = effect->directionY;
#if MAX_AXIS == 3
	effect_p->directionZ = effect->directionZ;
#endif
	if(effect->duration == 0)
	{ // Fix for games assuming 0 is infinite
		pEffect->duration = FFB_EFFECT_DURATION_INFINITE;
	}
	else
	{
		pEffect->duration = effect->duration;
	}
//	effect_p->startDelay = effect->startDelay;
	if(!ffb->ffb_active)
		HidFFB_start_FFB(ffb);

//	HidFFB_sendStatusReport(ffb,effect->effectBlockIndex); // TODO required?
}

/*
	Set Condition Report
 */
void HidFFB_set_condition(HidFFB* ffb,FFB_SetCondition_Data_t *cond)
{
	if(cond->effectBlockIndex == 0 || cond->effectBlockIndex > MAX_EFFECTS)
	{
		return;
	}
	uint8_t axis = cond->parameterBlockOffset;
	if (axis >= MAX_AXIS)
	{
		return; // sanity check!
	}
	FFB_Effect *pEffect = &ffb->effects[cond->effectBlockIndex - 1];
	pEffect->conditions[axis].cpOffset = cond->cpOffset;
	pEffect->conditions[axis].negativeCoefficient = cond->negativeCoefficient;
	pEffect->conditions[axis].positiveCoefficient = cond->positiveCoefficient;
	pEffect->conditions[axis].negativeSaturation = cond->negativeSaturation;
	pEffect->conditions[axis].positiveSaturation = cond->positiveSaturation;
	pEffect->conditions[axis].deadBand = cond->deadBand;
	//effect->conditionsCount++;

	if(pEffect->conditions[axis].positiveSaturation == 0)
	{
		pEffect->conditions[axis].positiveSaturation = 0x7FFF;
	}
	if(pEffect->conditions[axis].negativeSaturation == 0)
	{
		pEffect->conditions[axis].negativeSaturation = 0x7FFF;
	}
}

/*
	Effect Operation Report
 */
void HidFFB_set_effect_operation(HidFFB* ffb,FFB_EffOp_Data_t* report){
	if(report->effectBlockIndex == 0 || report->effectBlockIndex > MAX_EFFECTS){
		return; // Invalid ID
	}
	// Start or stop effect
	uint8_t id = report->effectBlockIndex-1;
	if(report->state == OP_EFFECT_STOP)
	{
		ffb->effects[id].state = 0; //Stop
	}
	else
	{
		// 1 = start, 2 = start solo
		if(report->state == OP_EFFECT_START_SOLO)
		{
			// Stop all other effects
			for(int i=0; i<MAX_EFFECTS;i++)
			{
				ffb->effects[i].state = 0;
			}

		}
		if(ffb->effects[id].state != 1)
		{
			HidFFB_set_filters(ffb,&ffb->effects[id]);
		}
		ffb->effects[id].startTime = HAL_GetTick() + ffb->effects[id].startDelay; // + effects[id].startDelay;
		ffb->effects[id].state = 1; //Start
		//TODO this parameter is unused now
		ffb->effects[id].loopCount = report->loopCount;
	}
//	HidFFB_sendStatusReport(ffb,id);
//	EC_logEffectState(ffb->effects_calc,ffb->effects[id].type,ffb->effects[id].state);
}

/*
	Set Envelope Report
 */
void HidFFB_set_envelope(HidFFB* ffb,FFB_SetEnvelope_Data_t *report){
	if(report->effectBlockIndex == 0 || report->effectBlockIndex > MAX_EFFECTS){
		return;
	}
	FFB_Effect *pEffect = &ffb->effects[report->effectBlockIndex - 1];

	pEffect->attackLevel = report->attackLevel;
	pEffect->attackTime = report->attackTime;
	pEffect->fadeLevel = report->fadeLevel;
	pEffect->fadeTime = report->fadeTime;
	pEffect->useEnvelope = true;
}

/*
	Set Ramp Force Report
 */
void HidFFB_set_ramp(HidFFB* ffb,FFB_SetRamp_Data_t *report){
	if(report->effectBlockIndex == 0 || report->effectBlockIndex > MAX_EFFECTS){
		return;
	}
	FFB_Effect *pEffect = &ffb->effects[report->effectBlockIndex - 1];
	pEffect->magnitude = 0x7fff; 		// Full magnitude for envelope calculation. This effect does not have a periodic report
	pEffect->startLevel = report->startLevel;
	pEffect->endLevel = report->endLevel;
}

/*
	Set Periodic Report
 */
void HidFFB_set_periodic(HidFFB* ffb,FFB_SetPeriodic_Data_t* report){
	if(report->effectBlockIndex == 0 || report->effectBlockIndex > MAX_EFFECTS){
		return;
	}
	FFB_Effect* pEffect = &ffb->effects[report->effectBlockIndex-1];
	pEffect->magnitude = report->magnitude;
	pEffect->offset = report->offset;
	pEffect->phase = report->phase;
	pEffect->period = report->period;
	CLIP(pEffect->period,1,0x7fff);			// Period is never 0
	//effect->counter = 0;
}

uint8_t HidFFB_find_free_effect(HidFFB* ffb,uint8_t type){ //Will return the first effect index which is empty or the same type
	for(uint8_t i=0;i<MAX_EFFECTS;i++){
		if(ffb->effects[i].type == FFB_EFFECT_NONE){
			return(i+1);
		}
	}
	return 0;
}

void HidFFB_reset_ffb(HidFFB* ffb){
	for(uint8_t i=0;i<MAX_EFFECTS;i++){
		HidFFB_free_effect(ffb,i);
	}
	//this->reportFFBStatus.effectBlockIndex = 1;
	ffb->reportFFBStatus.status = (HID_ACTUATOR_POWER) | (HID_ENABLE_ACTUATORS);
//	ffb->used_effects = 1;
}













